#include "PCX03Data.h"

#include <stdlib.h>
#include <string>
#include <sstream>
#include <iomanip>

// Joystick data
#define DEFAULT_JOYSTICK_AXIS_MIN -1.
#define DEFAULT_JOYSTICK_AXIS_MAX 1.
#define JOYSTICK_X_MIN DEFAULT_JOYSTICK_AXIS_MIN
#define JOYSTICK_X_MAX DEFAULT_JOYSTICK_AXIS_MAX
#define DEFAULT_JOYSTICK_X (JOYSTICK_X_MAX + JOYSTICK_X_MIN) / 2.
#define JOYSTICK_Y_MIN DEFAULT_JOYSTICK_AXIS_MIN
#define JOYSTICK_Y_MAX DEFAULT_JOYSTICK_AXIS_MAX
#define DEFAULT_JOYSTICK_Y (JOYSTICK_Y_MAX + JOYSTICK_Y_MIN) / 2.
#define JOYSTICK_Z_MIN DEFAULT_JOYSTICK_AXIS_MIN
#define JOYSTICK_Z_MAX DEFAULT_JOYSTICK_AXIS_MAX
#define DEFAULT_JOYSTICK_Z (JOYSTICK_Z_MAX + JOYSTICK_Z_MIN) / 2.
#define THROTTLE_MIN 0.
#define THROTTLE_MAX 1.
#define DEFAULT_THROTTLE THROTTLE_MIN

#define JOYSTICK_AXES_DECIMALS 3
#define JOYSTICK_X_DECIMALS JOYSTICK_AXES_DECIMALS
#define JOYSTICK_Y_DECIMALS JOYSTICK_AXES_DECIMALS
#define JOYSTICK_Z_DECIMALS JOYSTICK_AXES_DECIMALS
#define THROTTLE_DECIMALS JOYSTICK_AXES_DECIMALS

PCX03Data::PCX03Data() 
    : AbstractData("PCX03"),
    m_joystickX(DEFAULT_JOYSTICK_X),
    m_joystickY(DEFAULT_JOYSTICK_Y), 
    m_joystickZ(DEFAULT_JOYSTICK_Z),
    m_throttle(DEFAULT_THROTTLE)
{}

int PCX03Data::SetJoystickX(const float& value)
{
    if (value > JOYSTICK_X_MAX)
    {
        m_joystickX = JOYSTICK_X_MAX;
        return 666;
    }
    else if (value < JOYSTICK_X_MIN)
    {
        m_joystickX = JOYSTICK_X_MIN;
        return -666;
    }
 
    m_joystickX = value;
    return 0;
}

int PCX03Data::SetJoystickY(const float& value)
{
    if (value > JOYSTICK_Y_MAX)
    {
        m_joystickY = JOYSTICK_Y_MAX;
        return 666;
    }
    else if (value < JOYSTICK_Y_MIN)
    {
        m_joystickY = JOYSTICK_Y_MIN;
        return -666;
    }
  
    m_joystickY = value;
    return 0;
}

int PCX03Data::SetJoystickZ(const float& value)
{
    if (value > JOYSTICK_Z_MAX)
    {
        m_joystickZ = JOYSTICK_Z_MAX;
        return 666;
    }
    else if (value < JOYSTICK_Z_MIN)
    {
        m_joystickZ = JOYSTICK_Z_MIN;
        return -666;
    }
 
    m_joystickZ = value;
    return 0;
}

int PCX03Data::SetThrottle(const float& value)
{
    if (value > THROTTLE_MAX)
    {
        m_throttle = THROTTLE_MAX;
        return 666;
    }
    else if (value < THROTTLE_MIN)
    {
        m_throttle = THROTTLE_MIN;
        return -666;
    }
 
    m_throttle = value;
    return 0;
}

/*
 * Data handling
 *
 * While calculating precision one should consider that:
 *  sign, and decimal dot, take away sentence space
 */
int PCX03Data::SerializeSentence(std::stringstream& stream, const bool& isSending)
{
     if (isSending)
     {
        // Send data to stream
        stream << m_numGeneratedSentences%100000000 << ','; // Ensure 8 bytes
        stream << std::setprecision(JOYSTICK_X_DECIMALS) << m_joystickX << ',';        
        stream << std::setprecision(JOYSTICK_Y_DECIMALS) << m_joystickY << ','; 
        stream << std::setprecision(JOYSTICK_Z_DECIMALS) << m_joystickZ << ',';
        stream << std::setprecision(THROTTLE_DECIMALS) << m_throttle;
     }
     else
     {
        char comma;
        unsigned long numGeneratedSentences;
        float joystickX, joystickY, joystickZ, throttle;
       
        // Recieve data from stream
        stream >> numGeneratedSentences >> comma;
        stream >> joystickX >> comma;
        stream >> joystickY >> comma; 
        stream >> joystickZ >> comma;
        stream >> throttle;

        // Set the values
        SetJoystickX(joystickX); 
        SetJoystickY(joystickY);
        SetJoystickZ(joystickZ);
        SetThrottle(throttle);
     }

     return 0;
}

/*
PCX03 Sentence format

1        10        20        30        40        50        60        70          82
|        |         |         |         |         |         |         |         | |
$PCX03,99999999,0.000000,0.000000,0.000000,0.000000*6A
 |     |        |        |        |        |        | 
 |     |	    |        |        |        |        Checksum data
 |     |	    |        |        |        |
 |     |	    |        |        |        Joystick Throttle [8 bytes]
 |     |	    |        |        |
 |     |	    |        |        Joystick Z axis [8 bytes]
 |     |	    |        |
 |     |	    |        Joystick Y axis [8 bytes]
 |     |	    |
 |     |	    Joystick X axis [8 bytes]
 |     |
 |     Sentence ID (Ordial number) [up to 8 bytes]
 |
 Ducktape Works Sentence sent from PC to for AV X3 [5 bytes]
*/